#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include <array>
#include <cmath>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Eigen>
#include "csv_reader.h"
#include "motion_model.h"
#include "trajectory_optimizer.h"

#define L 1.0
#define DS 0.1
#define CONST_V 3.0  // use a const linear velocity here
typedef std::vector<std::vector<float>> Table;

// 产生车辆状态的path_points: (x,y,yaw)
StateList sample_states(std::vector<float> angle_samples, float a_min, float a_max, int d, 
                                                        float p_max, float p_min, int nh) {
  StateList states; // 状态的list
  for (float item : angle_samples) {
    float a = a_min + (a_max - a_min) * item; // 角度
    for (int j = 0; j < nh; j++)
    {
      float xf = d * std::cos(a); // x
      float yf = d * std::sin(a); // y
      float yawf;
      if (nh == 1)
        yawf = (p_max - p_min) / 2.0 + a;
      else
        yawf = p_min + (p_max - p_min) * j / (nh - 1) + a;
      states.push_back( TrajState(xf, yf, yawf) );
    }
  }
  return states;
};

// 一种state_lattice的形式
StateList calc_uniform_polar_states(int nxy, int nh, int d, float a_min, float a_max,
                                                            float p_min, float p_max)
{
  std::vector<float> angle_samples;
  for (int i = 0; i < nxy; i++)
  {
    angle_samples.push_back(i * 1.0 / (nxy - 1)); // 位置方向的占比:1/4,2/4,3/4等
  }
  StateList states = sample_states(angle_samples, a_min, a_max, d, p_max, p_min, nh);
  return states;
};

// 找到 lookuptable.csv中距离当前状态最近的参数
Parameter search_nearest_one_from_lookuptable(TrajState target, Table csv_file) {
  float min_d = std::numeric_limits<float>::max();
  int min_id = -1;
  for (unsigned int i = 0; i < csv_file.size(); i++)
  {
    float dx = target.x - csv_file[i][0];
    float dy = target.y - csv_file[i][1];
    float dyaw = target.yaw - csv_file[i][2];
    float d = std::sqrt(dx * dx + dy * dy + dyaw * dyaw);
    if (d < min_d)
    {
      min_id = i;
      min_d = d;
    }
  }
  Parameter best_p(std::sqrt(target.x * target.x + target.y * target.y),
                   {{0, csv_file[min_id][4], csv_file[min_id][5]}}); // ( distance,{0,km,kf} )
  return best_p;
}

// 产生path_points
std::vector<Traj> generate_path(StateList states, Table csv_file, float k0)
{
  std::vector<Traj> traj_list;
  // 对于每一个state进行循环
  for (TrajState state : states)
  {
    // 找到 lookuptable.csv中距离当前状态最近的参数
    Parameter p = search_nearest_one_from_lookuptable(state, csv_file);
    p.steering_sequence[0] = k0;
    // (x,y,yaw,v)
    State init_state(0, 0, 0, CONST_V);
    // 车辆运动学模型
    MotionModel m_model(L, DS, init_state);
    float cost_th_ = 0.1;
    std::vector<float> h_step_{0.5, 0.02, 0.02};
    int max_iter = 100;
    // 实例化TrajectoryOptimizer，计算trajectory
    TrajectoryOptimizer traj_opti_obj(m_model, p, state);
    Traj best_trajectory = traj_opti_obj.optimizer_Traj(max_iter, cost_th_, h_step_);
    // opencv 可视化
    cv::Mat bg(5000, 5000, CV_8UC3, cv::Scalar(255, 255, 255));
    if (true){
      cv::namedWindow("mptg", cv::WINDOW_NORMAL);
    }
    for (unsigned int i = 1; i < best_trajectory.size(); i++)
    {
      cv::line(bg, cv_offset(best_trajectory[i - 1].x, best_trajectory[i - 1].y, bg.cols, bg.rows),
          cv_offset(best_trajectory[i].x, best_trajectory[i].y, bg.cols, bg.rows),cv::Scalar(0, 0, 0),10);
    }
    cv::circle(bg, cv_offset(state.x, state.y, bg.cols, bg.rows),30, cv::Scalar(255, 0, 0), 5);
    cv::circle(bg, cv_offset(m_model.state.x, m_model.state.y, bg.cols, bg.rows),30,cv::Scalar(0, 0, 255), 5);
    cv::arrowedLine(bg, cv_offset(state.x, state.y, bg.cols, bg.rows), cv_offset(state.x + std::cos(state.yaw), 
                    state.y + std::sin(state.yaw), bg.cols, bg.rows),cv::Scalar(255, 0, 255),15);
    if (true)
    {
      cv::imshow("mptg", bg);
      cv::waitKey(5);
    }
    // end of opencv visualization
    traj_list.push_back(best_trajectory);
  }
  return traj_list;
};


void uniform_terminal_state_sample_test(Table csv_file)
{
  float k0 = 0.0;
  int nxy = 5;                      // number of position sampling
  int nh = 3;                       // number of heading sampling
  int d = 20;                       // distance to target
  float a_min = -45.0 / 180 * M_PI; // position sampling min angle
  float a_max = +45.0 / 180 * M_PI; // position sampling max angle
  float p_min = -45.0 / 180 * M_PI; // heading sampling min angle
  float p_max = +45.0 / 180 * M_PI; // heading sampling max angle

  StateList states = calc_uniform_polar_states(nxy, nh, d, a_min, a_max, p_min, p_max);
  std::vector<Traj> traj_list = generate_path(states, csv_file, k0);
  };

int main()
{
  // lookuptable.cpp: x,y,yaw,s,km,kf
  std::vector<std::vector<float>> lookup_table;
  // 使用绝对路径为了vscode debug
  std::ifstream file("/home/zy/Desktop/State_Lattice_Planner/lookuptable.csv");
  // 读入.csv文件
  CSVIterator loop(file);
  loop++;
  for (; loop != CSVIterator(); ++loop)
  {
    std::vector<float> temp;
    for (int i = 0; i < 6; i++)
    {
      temp.push_back(std::stod((*loop)[i]));
    }
    lookup_table.push_back(temp);
  }
  // 可以使用不同的state_lattice的形式
  uniform_terminal_state_sample_test(lookup_table);
};